#include <iostream>
#include "navigation/cmd_publisher/twist_publisher.hpp"

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("demo_twist");

    TwistPublisher twist_publisher(node);

    RCLCPP_INFO(node->get_logger(), "Start robot walking...");
    twist_publisher.walk(3.0); // Ensure the robot is in walk mode

    // RCLCPP_INFO(node->get_logger(), "Moving forward for 3 seconds...");
    // twist_publisher.set_twist(0.3, 0.0, 0.0, 3.0); // Move forward for 5 seconds
    // RCLCPP_INFO(node->get_logger(), "Stopping for 3 seconds...");
    // twist_publisher.set_twist(0.0, 0.0, 0.0, 3.0); // Stop for 3 seconds
    // RCLCPP_INFO(node->get_logger(), "Stance for 4 seconds...");
    // twist_publisher.stance(4.0); // Stance for 4 seconds

    // RCLCPP_INFO(node->get_logger(), "Moving back for 3 seconds...");
    // twist_publisher.set_twist(-0.3, 0.0, 0.0, 3.0); // Move back for 3 seconds
    // RCLCPP_INFO(node->get_logger(), "Stance for 5 seconds...");
    // twist_publisher.stance(5.0); // Stance for 5 seconds

    // while (1) {
    //     RCLCPP_INFO(node->get_logger(), "Moving left for 5 seconds...");
    //     twist_publisher.set_twist(0.0, 0.1, 0.0, 5.0); // Move left for 5 seconds
    //     RCLCPP_INFO(node->get_logger(), "Stance for 4 seconds...");
    //     twist_publisher.stance(4.0); // Stance for 4 seconds

    //     RCLCPP_INFO(node->get_logger(), "Moving right for 5 seconds...");
    //     twist_publisher.set_twist(0.0, -0.1, 0.0, 5.0); // Move right for 5 seconds
    //     RCLCPP_INFO(node->get_logger(), "Stance for 4 seconds...");
    //     twist_publisher.stance(4.0); // Stance for 4 seconds

    // }

    // RCLCPP_INFO(node->get_logger(), "Rotating ... [Enter to stop]");
    // twist_publisher.set_twist(0.0, 0.0, 0.2);

    // std::cin.get(); // Wait for user input to stop rotation
    RCLCPP_INFO(node->get_logger(), "Shutting down, stopping the robot...");
    twist_publisher.stance(1.0); // Ensure the robot goes to stance

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}